/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.mechanism.breakaway;

import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Command;
import com.grt192.core.Mechanism;
import com.grt192.event.component.AccelerometerEvent;
import com.grt192.event.component.AccelerometerListener;
import com.grt192.event.component.EncoderEvent;
import com.grt192.event.component.EncoderListener;
import com.grt192.event.component.GyroEvent;
import com.grt192.event.component.GyroListener;
import com.grt192.sensor.GRTAccelerometer;
import com.grt192.sensor.GRTEncoder;
import com.grt192.sensor.GRTGyro;

/**
 *
 * @author Bonan, James
 */
public class GRTBreakawayRobotBase extends Mechanism implements EncoderListener, GyroListener,
AccelerometerListener{

    public static final int TANK_DRIVE = 0;
    public static final int ONE_STICK_DRIVE = 1;
    public static final int CAR_DRIVE = 2;
    public static final double GOAL1X = 2.0;
    public static final double GOAL1Y = 0.0;
    public static final double GOAL2X = 25;
    public static final double GOAL2Y = 0.0;
    public final double S_C_BASE_VALUE = 0.5; //Input at which motor starts moving
    private boolean automatedDrive = false; // becomes true when it is going
    //through a automated sequence (i.e.
    //going over bump)
    //TODO keep track of position
//    private double prevAngle;
    private double angle = 0.0;
    private double prevLeftDist = 0.0;
    private double prevRightDist = 0.0;
    private double leftDist = 0.0;
    private double rightDist = 0.0;
    private double posX = 0.0;
    private double posY = 0.0;
    public static final double ZONE1STARTX = 0.0; //pre-determined (these are random)
    public static final double ZONE1STARTY = 0.0;
    public static final double ZONE2STARTX = 7.0;
    public static final double ZONE2STARTY = 16.0;
    public static final double ZONE3STARTX = 7.0;
    public static final double ZONE3STARTY = 53.0;
    public static int zone = 1; //starting zone
    private double gyroWarmUpSum = 0.0;
    private int gyroCount = 0;
    public final double GYRO_TOTAL_WARM_UP_COUNT = 200.0;
    private double gyroWarmUpAverage = 0.0;
    private double prevAngle;
    private double axWarmUpSum = 0.0;
    private int axCount = 0;
    public final double AX_TOTAL_WARM_UP_COUNT = 200.0;
    private double axWarmUpAverage = 0.0;


    //the entire robot base, including input and output...
    public GRTBreakawayRobotBase(GRTJaguar leftJaguar1, GRTJaguar leftJaguar2,
            GRTJaguar rightJaguar1, GRTJaguar rightJaguar2,
            GRTAccelerometer ax1, GRTGyro gy1,
            GRTEncoder leftEn, GRTEncoder rightEn) {

        leftJaguar1.start();
        leftJaguar2.start();
        rightJaguar1.start();
        rightJaguar2.start();

        ax1.start();
        gy1.start();
        gy1.addGyroListener(this);
        leftEn.start();
        //leftEn.addEncoderListener(this);
        rightEn.start();
        //rightEn.addEncoderListener(this);

        addActuator("LeftJaguar1", leftJaguar1);
        addActuator("LeftJaguar2", leftJaguar2);
        addActuator("RightJaguar1", rightJaguar1);
        addActuator("RightJaguar2", rightJaguar2);

        addSensor("AccelerometerX", ax1);
        addSensor("Gyro", gy1);
        addSensor("LeftEncoder", leftEn);
        addSensor("RightEncoder", rightEn);

        if (zone == 1) {
            posX = ZONE1STARTX;
            posY = ZONE1STARTY;
        }
        else if (zone == 2) {
            posX = ZONE2STARTX;
            posY = ZONE2STARTY;
        }
        else if (zone == 3) {
            posX = ZONE3STARTX;
            posY = ZONE3STARTY;
        }
    }

    public double getAccelerationX() {
        return getSensor("AccelerometerX").getState("Acceleration");
    }

    public double getLeftDist() {
        return ((GRTEncoder) getSensor("LeftEncoder")).getState("Distance");
    }

    public double getRightDist() {
        return ((GRTEncoder) getSensor("RightEncoder")).getState("Distance");
    }

    public double getGyroAngle() {
        return getSensor("Gyro").getState("Angle");
    }

    public double getAngle() {
        return angle;
    }

    public void overrideAutomated() {
        automatedDrive = false;
    }

    public void tankDrive(double leftValue, double rightValue) {
        // the negative sign is for making it so that the joystick isn't flipped
        if (!automatedDrive) {
            Command c1 = new Command(leftValue, 0);
            Command c2 = new Command(rightValue, 0);
            getActuator("LeftJaguar1").enqueueCommand(c1);
            getActuator("LeftJaguar2").enqueueCommand(c1);
            getActuator("RightJaguar1").enqueueCommand(c2);
            getActuator("RightJaguar2").enqueueCommand(c2);
        }

    }

    public void carDrive(double x, double y) {
        carDrive(x, y, false);
    }

    public void carDrive(double x, double y, boolean spin) {
        double leftSpeed = x;
        double rightSpeed = x;
        if (y < 0) {
            leftSpeed *= (spin? 1/x : -1) * y;
        } else if (y > 0) {
            rightSpeed *= (spin? -1/x : 1) * y;
        }
        tankDrive(leftSpeed, -rightSpeed);
    }
    
    public void oneStickDrive(double magnitude, double angle) { //point turn
        if (!automatedDrive) {
            double turnAngle = turn(angle, getAngle());
            Command c1;
            Command c2;

            double positiveSpeed = S_C_BASE_VALUE + (1 - S_C_BASE_VALUE) * magnitude;
            double negativeSpeed = S_C_BASE_VALUE + (1 - S_C_BASE_VALUE) * magnitude;

            if (turnAngle > 0) {
                c1 = new Command(positiveSpeed, 0);
                c2 = new Command(negativeSpeed, 0);
            } else {
                if (turnAngle < 0) {
                    c1 = new Command(negativeSpeed, 0);
                    c2 = new Command(positiveSpeed, 0);
                } else {
                    c1 = new Command(0, 0);
                    c2 = new Command(0, 0);
                }
            }

            getActuator("LeftJaguar1").enqueueCommand(c1);
            getActuator("LeftJaguar2").enqueueCommand(c1);
            getActuator("RightJaguar1").enqueueCommand(c2);
            getActuator("RightJaguar2").enqueueCommand(c2);
        }
    }

    public void overTheBump() {
        automatedDrive = true;
        Command c1 = new Command(1, 0);
        Command c2 = new Command(1, 0);
        getActuator("LeftJaguar1").enqueueCommand(c1);
        getActuator("LeftJaguar2").enqueueCommand(c1);
        getActuator("RightJaguar1").enqueueCommand(c2);
        getActuator("RightJaguar2").enqueueCommand(c2);
        try {
            Thread.sleep(1000);
        } catch (InterruptedException ex) {
            ex.printStackTrace();
        }
        if (automatedDrive) {
            Command c3 = new Command(1, 0);
            Command c4 = new Command(1, 0);
            getActuator("LeftJaguar1").enqueueCommand(c3);
            getActuator("LeftJaguar2").enqueueCommand(c3);
            getActuator("RightJaguar1").enqueueCommand(c4);
            getActuator("RightJaguar2").enqueueCommand(c4);
            try {
                Thread.sleep(1000);
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }
        }
        if (automatedDrive) {
            Command c3 = new Command(1, 0);
            Command c4 = new Command(1, 0);
            getActuator("LeftJaguar1").enqueueCommand(c3);
            getActuator("LeftJaguar2").enqueueCommand(c3);
            getActuator("RightJaguar1").enqueueCommand(c4);
            getActuator("RightJaguar2").enqueueCommand(c4);
            try {
                Thread.sleep(1000);
            } catch (InterruptedException ex) {
                ex.printStackTrace();
            }
        }
        automatedDrive = false;
    }

   public void updatePos() //angle in degrees, airplane style (this is how the gyro is)
    {
        posX += Math.sin(angle * Math.PI / 180.0) * ((leftDist - prevLeftDist) +
                (rightDist - prevRightDist)) / 2.0;
        posY += Math.cos(angle * Math.PI / 180.0) * ((leftDist - prevLeftDist) +
                (rightDist - prevRightDist)) / 2.0;

    }

    private static double turn(double start, double end) {
        end -= start; //shift start to 0, and end accordingly
        end += end < 0 ? 360 : 0; //make end angle positve
        return end > 180 ? end - 360 : end; //pick the shorter side
    }

    public void countDidChange(EncoderEvent e) {
        
    }

    public double getX() //x position on field
    {
        return posX;
    }

    public double getY() //y position on field
    {
        return posY;
    }

    public void rotationDidStart(EncoderEvent e) {
    }

    public void rotationDidStop(EncoderEvent e) {
    }

    public void changedDirection(EncoderEvent e) {
    }

    public void didReceiveAngle(GyroEvent e) {

    }

    public void didAngleChange(GyroEvent e) {

    }

    public void didAngleSpike(GyroEvent e) {

    }

    public void didReceiveAcceleration(AccelerometerEvent e) {
        
    }

    public void didAccelerationSpike(AccelerometerEvent e) {
    }

    public void didAccelerationChange(AccelerometerEvent e) {
    }
}
